#include "pso_utils.h"


void draw_swarm(
	const std::vector<PSO::Particle*>& swarm,
	CImg<unsigned char>&               canvas,
	const std::vector<double>*         position
	)
{
	double tx,ty,sx,sy,r1,r2;
	unsigned char red[] = {255,0,0}, blue[] = {0,0,255};

	for (unsigned int i=0; i<swarm.size(); i++) {
		tx = swarm[i]->get_position(0);
		ty = swarm[i]->get_position(1);
		sx = swarm[i]->get_position(2);
		sy = swarm[i]->get_position(3);

		if ( (int)tx<0 || (int)tx>=canvas.width() || 
				(int)ty<0 || (int)ty>=canvas.height())
			continue;

		r1 = (6.0*sx < 1.0) ? 1.0: 6.0*sx;
		r2 = (6.0*sy < 1.0) ? 1.0: 6.0*sy;
		canvas.draw_ellipse((int)tx, (int)ty, r1, r2, 0, red);
		//canvas.draw_circle((int)tx,(int)ty,1,red);
	}
	
	if (position != NULL) {
		tx = (*position)[0];
		ty = (*position)[1];
		sx = (*position)[2];
		sy = (*position)[3];

		if ( !( (int)tx<0 || (int)tx>=canvas.width() || 
				(int)ty<0 || (int)ty>=canvas.height() ) ) {
			r1 = (6.0*sx < 1.0) ? 1.0: 6.0*sx;
			r2 = (6.0*sy < 1.0) ? 1.0: 6.0*sy;			
			canvas.draw_ellipse((int)tx, (int)ty, r1, r2, 0, blue);
		}
	}
}


void draw_linechart(
	const std::vector<double>&                   position,
	const std::vector<std::pair<double,double>>& linechart_in,
	CImg<unsigned char>&                         canvas
	)
{
	double tx,ty,sx,sy;
	const unsigned char blue[]={0,0,255};

	tx = position[0];
	ty = position[1];
	sx = position[2];
	sy = position[3];

	double x0 = (linechart_in[0].first * sx) + tx;
	double y0 = (linechart_in[0].second * sy) + ty;

	if ((int)x0<0 || (int)x0>=canvas.width()  || 
		(int)y0<0 || (int)y0>=canvas.height() ) {
			return;
	}

	for (unsigned int i=1; i<linechart_in.size(); i++) {
		double x1 = (linechart_in[i].first * sx) + tx;
		double y1 = (linechart_in[i].second * sy) + ty;

		if ((int)x1<0 || (int)x1>=canvas.width()  || 
			(int)y1<0 || (int)y1>=canvas.height()) {
				return;
		} else {
			canvas.draw_line((int)x0,(int)y0,(int)x1,(int)y1,blue);
		}
		x0 = x1; y0 = y1;
	}
}